#modified with a while loop

# contains the functions which control the higher level responses of
# the robot, requires the robot class as a constructor 
# note: all the idm functions calculate the actions for 1 cycle

#require the math library
require 'gui_control_lib/math/mathmodule.rb'
include Mathmodule

class Idm
  
  def initialize(robotclass)
     @robotclass = robotclass
     
     #mazemovefunctions
     @mazerunning=0
  end  
 
  

  def smartmove
#start by backing up a bit
checkno=20
cbot=@robotclass  
cbot.smartback(0.2,2.0) if (cbot.col_frontcheck)
#find max 
scans=cbot.rangescan.size
maxscans=Array.new()
maxdeg=Array.new()
scanarray=Array.new(cbot.rangescan)
  
checkno.times{
max=0.0
index=-1
scanarray.each_with_index{|rangestr,deg|
  range=rangestr.to_f
  if(range>max and range<19.9)
    max=range
    index=deg
  end
 }
scanarray.delete_at(index) #remove from list
maxscans<<max
maxdeg<<(index)
}

#create roulette wheel
roullet=Array.new(maxscans)

#find sum
sum=0.0
roullet.each{|i|
  sum=sum+i
}

#change to percents 
roullet.collect!{|member|
  member=(member/sum)*100
}

#change to sumation of percents
sum=0.0
roullet.collect!{|member|
  member=member+sum
  sum=member
}

#randomize and choose
choice=rand(100)
choicedeg=0.0
roullet.each_with_index {|member,index|
    if(choice<member)
      choicedeg=maxdeg[index]
      choice=maxscans[index]
      break
    end
}
#the chosen deg is choicedeg and distance is choice
puts choice,choicedeg 
#back up a bit

if(choicedeg<=90)
  cbot.smartright(de2rad(90-choicedeg))
else
  cbot.smartleft(de2rad(choicedeg-90))
end

#puts choice
dist=(choice-0.5)

#cbot.smartfor(dist,2.0)
cbot.smartfor(1.0,2.0)
sleep(1.0)
  end
  
  def smartmoveloop
   Thread.new{   
    Thread.current["name"]="idm_#{@robotclass.robotname}_smartmoveloop"
    while (true)
       self.smartmove()
       sleep(0.1)
    end
    }
  end
  
  
  #orders the robot to move around randomly.
  # Note:Only usable with the wheeled robots, P2AT etc.
  # Note:The Robot won't use it's sensors and will not care if it hits anything
  def randommove
   rnum= rand(10)
 if rnum<8
    out ="DRIVE {Left 2.0} {Right 2.0} \r\n"
  end 
  if rnum==8
    out ="DRIVE {Left -2.0} {Right -2.0}\r\n"
  end 
  if rnum==9
   out ="DRIVE {Left 2.0} {Right -2.0} \r\n"
  end 
  if rnum==10
  out ="DRIVE {Left -2.0} {Right 2.0}\r\n"
  end 
    #@connect.send(out)
    @connect.write(out)
  end
  
  
  
  def mazemove2
  Thread.new{
   Thread.current["name"]="idm_#{@robotclass.robotname}_mazemove2command" 
   if(@mazerunning==0)
     @mazerunning=1
    if(@robotclass.static_col==1)
       puts "collision detected"
       @robotclass.smartback(0.5,1.0)
       @robotclass.smartleft(6.28/4)
    end
    if(@robotclass.col_frontcheck)  
     puts "sensor_front collision detected"
      if(rand(10)<5)
       while(@robotclass.col_frontcheck)
        @robotclass.smartleft(0.5)
       end
      else
       while(@robotclass.col_frontcheck)
         @robotclass.smartright(0.5)
       end
      end

    else 
      @robotclass.smartfor(0.3,2.0)
    end
    @mazerunning=0
  end
      
  } 
  end
    
  def mazemove2loop
   Thread.new{   
     Thread.current["name"]="idm_#{@robotclass.robotname}_mazemove2loop"
    while (true)
       self.mazemove2
       sleep(0.1)
    end
    }
  end

 

  
end